
// import haxe.ds.Array;
// import oimo.common.Setting;
// import oimo.common.Vec3;
// import oimo.dynamics.constraint.*;
// import oimo.dynamics.rigidbody.*;
// import oimo.m.Vec3;
// import oimo.m.M;

import { Vec3 } from "../common/Vec3";
import { RigidBody } from "./rigidbody/RigidBody";
import { ConstraintSolver } from "./constraint/ConstraintSolver";
import { Setting } from "../common/Setting";
import { M } from "../common/M";
import { PositionCorrectionAlgorithm } from "./constraint/PositionCorrectionAlgorithm";
import { TimeStep } from "./TimeStep";
import { RigidBodyType } from "./rigidbody/RigidBodyType";

/**
 * Simulation island.
 */


export class Island {
	gravity: Vec3;

	numRigidBodies: number;
	rigidBodies: Array<RigidBody>;

	// all the constraint solvers
	numSolvers: number;
	solvers: Array<ConstraintSolver>;

	// the constraint solvers use split impulse for position part
	numSolversSi: number;
	solversSi: Array<ConstraintSolver>;

	// the constraint solvers use nonlinear Gauss-Seidel for position part
	numSolversNgs: number;
	solversNgs: Array<ConstraintSolver>;

	constructor() {
		this.rigidBodies = new Array<RigidBody>(Setting.islandInitialRigidBodyArraySize);
		this.solvers = new Array<ConstraintSolver>(Setting.islandInitialConstraintArraySize);
		this.solversSi = new Array<ConstraintSolver>(Setting.islandInitialConstraintArraySize);
		this.solversNgs = new Array<ConstraintSolver>(Setting.islandInitialConstraintArraySize);
		this.numRigidBodies = 0;
		this.numSolvers = 0;
		this.numSolversSi = 0;
		this.numSolversNgs = 0;
		this.gravity = new Vec3();
	}

	// --- ---


	private fastInvExp(x: number): number {
		var x2: number = x * x;
		return 1 / (1 + x + x2 * (1 / 2 + x * (1 / 6) + x2 * (1 / 24)));
	}


	private addConstraintSolverSI(solver: ConstraintSolver): void {
		this.solversSi[this.numSolversSi++] = solver;
	}


	private addConstraintSolverNgs(solver: ConstraintSolver): void {
		this.solversNgs[this.numSolversNgs++] = solver;
	}

	// --- internal ---

	public _clear(): void {
		this.rigidBodies = new Array<RigidBody>(Setting.islandInitialRigidBodyArraySize);
		this.solvers = new Array<ConstraintSolver>(Setting.islandInitialConstraintArraySize);
		this.solversSi = new Array<ConstraintSolver>(Setting.islandInitialConstraintArraySize);
		this.solversNgs = new Array<ConstraintSolver>(Setting.islandInitialConstraintArraySize);
		this.numRigidBodies = 0;
		this.numSolvers = 0;
		this.numSolversSi = 0;
		this.numSolversNgs = 0;
	}


	public _setGravity(gravity: Vec3): void {
		M.vec3_fromVec3(this.gravity, gravity);
	}

	public _addRigidBody(rigidBody: RigidBody): void {
		rigidBody._addedToIsland = true;
		this.rigidBodies[this.numRigidBodies++] = rigidBody;
	}

	public _addConstraintSolver(solver: ConstraintSolver, positionCorrection: number): void {
		solver._addedToIsland = true;
		this.solvers[this.numSolvers++] = solver;

		if (positionCorrection == PositionCorrectionAlgorithm.SPLIT_IMPULSE) {
			this.addConstraintSolverSI(solver);
		}
		if (positionCorrection == PositionCorrectionAlgorithm.NGS) {
			this.addConstraintSolverNgs(solver);
		}
	}

	// steps the single rigid body
	public _stepSingleRigidBody(timeStep: TimeStep, rb: RigidBody): void {
		var dt: number = timeStep.dt;

		// store previous transform
		M.transform_assign(rb._ptransform, rb._transform);

		// update sleep time
		if (rb._isSleepy()) {
			rb._sleepTime += dt;
			if (rb._sleepTime > Setting.sleepingTimeThreshold) {
				rb.sleep();
			}
		} else {
			rb._sleepTime = 0;
		}

		if (!rb._sleeping) {
			// the rigid body is awake
			if (rb._type == RigidBodyType.DYNAMIC) {
				// damping
				var linScale: number = this.fastInvExp(dt * rb._linearDamping);
				var angScale: number = this.fastInvExp(dt * rb._angularDamping);

				// compute accelerations
				let linAcc: Vec3 = new Vec3();
				let angAcc: Vec3 = new Vec3();
				M.vec3_scale(linAcc, this.gravity, rb._gravityScale);
				M.vec3_addRhsScaledEQ(linAcc, linAcc, rb._force, rb._invMass);
				M.vec3_mulMat3(angAcc, rb._torque, rb._invInertia);

				// update velocity
				M.vec3_addRhsScaledEQ(rb._vel, rb._vel, linAcc, dt);
				M.vec3_scale(rb._vel, rb._vel, linScale);
				M.vec3_addRhsScaledEQ(rb._angVel, rb._angVel, angAcc, dt);
				M.vec3_scale(rb._angVel, rb._angVel, angScale);
			}
			rb._integrate(dt);
			rb._syncShapes();
		}
	}

	// steps the island with multiple bodies and constraints
	public _step(timeStep: TimeStep, numVelocityIterations: number, numPositionIterations: number): void {
		var dt: number = timeStep.dt;

		var sleepIsland: boolean = true;

		// sleep check and apply gravity
		for (let i = 0; i < this.numRigidBodies; ++i) {
			var rb: RigidBody = this.rigidBodies[i];

			// store previous transform
			M.transform_assign(rb._ptransform, rb._transform);

			// don't let the rigid body sleep
			rb._sleeping = false;

			// update sleep time
			if (rb._isSleepy()) {
				rb._sleepTime += dt;
			} else {
				rb._sleepTime = 0;
			}

			// check if the rigid body is awaken
			if (rb._sleepTime < Setting.sleepingTimeThreshold) {
				// awaken the whole island
				sleepIsland = false;
			}

			// apply forces
			if (rb._type == RigidBodyType.DYNAMIC) {
				// damping
				var linScale: number = this.fastInvExp(dt * rb._linearDamping);
				var angScale: number = this.fastInvExp(dt * rb._angularDamping);

				// compute accelerations
				var linAcc: Vec3 = new Vec3();
				var angAcc: Vec3 = new Vec3();
				M.vec3_scale(linAcc, this.gravity, rb._gravityScale);
				M.vec3_addRhsScaledEQ(linAcc, linAcc, rb._force, rb._invMass);
				M.vec3_mulMat3(angAcc, rb._torque, rb._invInertia);

				// update velocity
				M.vec3_addRhsScaledEQ(rb._vel, rb._vel, linAcc, dt);
				M.vec3_scale(rb._vel, rb._vel, linScale);
				M.vec3_addRhsScaledEQ(rb._angVel, rb._angVel, angAcc, dt);
				M.vec3_scale(rb._angVel, rb._angVel, angScale);
			}
		}

		if (sleepIsland) {
			// sleep the whole island
			for (let i = 0; i < this.numRigidBodies; ++i) {
				var rb: RigidBody = this.rigidBodies[i];
				rb.sleep();
			}
			return;
		}


		// -------------- test --------------

		/*
		// randomize constraint order

		for (i in 1...numSolvers) {
			var j = Std.int(Math.random() * (i + 1));
			var tmp = solvers[i];
			solvers[i] = solvers[j];
			solvers[j] = tmp;
		}

		for (i in 1...numSolversSi) {
			var j = Std.int(Math.random() * (i + 1));
			var tmp = solversSi[i];
			solversSi[i] = solversSi[j];
			solversSi[j] = tmp;
		}

		for (i in 1...numSolversNgs) {
			var j = Std.int(Math.random() * (i + 1));
			var tmp = solversNgs[i];
			solversNgs[i] = solversNgs[j];
			solversNgs[j] = tmp;
		}
		*/

		// -------------- test --------------

		// solve velocity
		for (let i = 0; i < this.numSolvers; ++i) {
			var s: ConstraintSolver = this.solvers[i];
			s.preSolveVelocity(timeStep);
		}
		for (let i = 0; i < this.numSolvers; ++i) {
			var s: ConstraintSolver = this.solvers[i];
			s.warmStart(timeStep);
		}
		for (let t = 0; t < numVelocityIterations; ++t) {
			for (let i = 0; i < this.numSolvers; ++i) {
				var s: ConstraintSolver = this.solvers[i];
				s.solveVelocity();
			}
		}

		// post-solve (velocity)
		for (let i = 0; i < this.numSolvers; ++i) {
			var s: ConstraintSolver = this.solvers[i];
			s.postSolveVelocity(timeStep);
		}

		// integrate
		for (let i = 0; i < this.numRigidBodies; ++i) {
			var rb: RigidBody = this.rigidBodies[i];
			rb._integrate(dt);
		}

		// solve split impulse
		for (let i = 0; i < this.numSolversSi; ++i) {
			var s: ConstraintSolver = this.solversSi[i];
			s.preSolvePosition(timeStep);
		}
		for (let t = 0; t < numPositionIterations; ++t) {
			for (let i = 0; i < this.numSolversSi; ++i) {
				var s: ConstraintSolver = this.solversSi[i];
				s.solvePositionSplitImpulse();
			}
		}

		// solve integrate pseudo velocity
		for (let i = 0; i < this.numRigidBodies; ++i) {
			var rb: RigidBody = this.rigidBodies[i];
			rb._integratePseudoVelocity();
		}

		// solve nonlinear Gauss-Seidel
		for (let i = 0; i < this.numSolversNgs; ++i) {
			var s: ConstraintSolver = this.solversNgs[i];
			s.preSolvePosition(timeStep);
		}
		for (let t = 0; t < numPositionIterations; ++t) {
			for (let i = 0; i < this.numSolversNgs; ++i) {
				var s: ConstraintSolver = this.solversNgs[i];
				s.solvePositionNgs(timeStep);
			}
		}

		// post-solve (some constraints may be removed)
		for (let i = 0; i < this.numSolvers; ++i) {
			var s: ConstraintSolver = this.solvers[i];
			s.postSolve();
		}

		// synchronize shapes
		for (let i = 0; i < this.numRigidBodies; ++i) {
			var rb: RigidBody = this.rigidBodies[i];
			rb._syncShapes();
		}
	}

}
